#pragma once
#include "opencv2/myopencv.hpp"
//#define VERBOSE

using namespace cv;

class Color2GrayMapper {
public:
	Mat intriMatC, intriMatG;
	Mat distortMatC, distortMatG;
	vector<Mat> rvecsC, rvecsG, tvecsC, tvecsG;

	Mat R,T,R1,T1,R2,T2;
	double k1,k2,k3,p1,p2;
	double edgeLen;

	int width, height;

	void load(void) {
		FileStorage fs;
		FileNode vecs;

		fs.open("calibrate_color_result.yml", FileStorage::READ);
		fs["intriMat"] >> intriMatC;
		fs["distortMat"] >> distortMatC;
		vecs = fs["size"] ;
		(*vecs.begin()) >> height;
		(*(++vecs.begin())) >> width;

		vecs = fs["rvecs"];
		for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
			Mat tmp;
			(*it) >> tmp;
			rvecsC.push_back(tmp);
		}
		vecs = fs["tvecs"];
		for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
			Mat tmp;
			(*it) >> tmp;
			tvecsC.push_back(tmp);
		}
		fs.release();


		fs.open("calibrate_gray_result.yml", FileStorage::READ);
		fs["intriMat"] >> intriMatG;
		fs["distortMat"] >> distortMatG;
		vecs = fs["rvecs"];
		for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
			Mat tmp;
			(*it) >> tmp;
			rvecsG.push_back(tmp);
		}
		vecs = fs["tvecs"];
		for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
			Mat tmp;
			(*it) >> tmp;
			tvecsG.push_back(tmp);
		}
		fs.release();

		fs.open("stereo_result.yml", FileStorage::READ);
		fs["R"] >> R;
		fs["T"] >> T;
		fs["edgeLen"] >> edgeLen;
		fs.release();

	}

	void init(void) {
		load();
		cout << "width: " << width << " height: " << height << endl;

		for (int i=7; i<8; ++i) {
			Rodrigues(rvecsC[i], R1);
			T1 = tvecsC[i];
// 			ezPrintMat(R1);
// 			ezPrintMat(T1);

			Rodrigues(rvecsG[i], R2);
			T2 = tvecsG[i];
// 			ezPrintMat(R2);
// 			ezPrintMat(T2);

// 			Mat R = R1.inv();
// 			Mat T = -R2*R*T1 + T2;
// 			R = R2*R;
// 
// 			ezPrintMat(R);
// 			ezPrintMat(T);
		}


		k1 = distortMatG.at<double>(0);
		k2 = distortMatG.at<double>(1);
		k3 = distortMatG.at<double>(4);
		p1 = distortMatG.at<double>(2);
		p2 = distortMatG.at<double>(3);
	}

	Mat mapping(Mat color, Mat dcMapped, Rect rect, double edgeInMM = -1){
		double scale = 263 * edgeLen/34;
		Mat mapped = Mat::zeros(height, width, CV_8UC3);
		for (int i=rect.y; i<rect.y+rect.height; ++i) {
			for (int j=rect.x; j<rect.x+rect.width; ++j) {
				short dval = dcMapped.at<short>(i,j);
				if (dval <= 100 || dval >= 10000) continue;

				Mat P = (Mat_<double>(3,1) << j,i,1);

				P = mapping(P, dval, scale);

				int px, py;
				px = cvRound(P.at<double>(0));
				py = cvRound(P.at<double>(1));

				if (0<=px && px<width && 0<=py && py<height) {
					mapped.at<Vec3b>(py,px) = color.at<Vec3b>(i,j);
				}
			}
		}
		return mapped;
	}

	Mat mapping(Mat color, Mat dcMapped, double scale = 263){
		Mat mapped = Mat::zeros(height, width, CV_8UC3);
		for (int i=0; i<height; ++i) {
			for (int j=0; j<width; ++j) {
				short dval = dcMapped.at<short>(i,j);
				if (dval <= 100 || dval >= 10000) continue;

				Mat P = (Mat_<double>(3,1) << j,i,1);

				P = mapping(P, dval, scale);

				int px, py;
				px = cvRound(P.at<double>(0));
				py = cvRound(P.at<double>(1));

				if (0<=px && px<width && 0<=py && py<height) {
					mapped.at<Vec3b>(py,px) = color.at<Vec3b>(i,j);
				}
			}
		}
		return mapped;
	}

	Mat mapping(Mat P, short dval, double scale = 263) {
#ifdef VERBOSE
		ezPrintMat(P);
		ezPrintMat(intriMatC.inv()*P);
#endif
		P = intriMatC.inv()*P;
#ifdef VERBOSE
		ezPrintMat(dval/26.1 * P);
#endif
		P = dval/scale * P;

#ifdef VERBOSE
		ezPrintMat(R*P+T);
#endif
		P = R*P+T;

#ifdef VERBOSE
		ezPrintMat(P/P.at<double>(2));
#endif
		P = P/P.at<double>(2);

		double x,y,r;
		x = P.at<double>(0);
		y = P.at<double>(1);
		r = x*x+y*y;
		P.at<double>(0) = x*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p1*x*y + p2*(r+2*x*x);
		P.at<double>(1) = y*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p2*x*y + p1*(r+2*y*y);

#ifdef VERBOSE
		ezPrintMat(intriMatG*P);
#endif
		P = intriMatG*P;
		return P;
	}

};
